﻿/******************************************************************************************
*                                                                                         *    
*                              Developed by Bruno Pires                                   *
*                                                                                         *
*                             email: bruno@blastersystems.com                             *
*                               web: www.blastersystems.com                               *
*                              blog: www.blastersystems.com/blog                          *
*                           twitter: @brunoacpires                                        *
*                                                                                         *
*             Software available under GNU LGPL v3 License without any guarantee          *
*                                                                                         *    
* *****************************************************************************************/ 
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenNI;
using System.Drawing;
using System.Windows.Media;
using System.Windows.Media.Imaging;

namespace Pap.Service.ImageSkl
{
    public class PapSkeleton
    {
        private Point3D LowerTorso;
        private string calibPose;
        public Point3D startingPoint;

        private UserGenerator userGenerator;
        private SkeletonCapability skeletonCapbility;
        private PoseDetectionCapability poseDetectionCapability;
        private Dictionary<int, Dictionary<SkeletonJoint, SkeletonJointPosition>> joints;

        public void StartDataProcessing(Context ctx)
        {
            userGenerator = new UserGenerator(ctx);
            skeletonCapbility = userGenerator.SkeletonCapability;
            poseDetectionCapability = userGenerator.PoseDetectionCapability;
            calibPose = skeletonCapbility.CalibrationPose;

            userGenerator.NewUser += userGenerator_NewUser;
            userGenerator.LostUser += userGenerator_LostUser;

            poseDetectionCapability.PoseDetected += poseDetectionCapability_PoseDetected;
            skeletonCapbility.CalibrationComplete += skeletonCapbility_CalibrationComplete;

            skeletonCapbility.SetSkeletonProfile(SkeletonProfile.All);
            joints = new Dictionary<int, Dictionary<SkeletonJoint, SkeletonJointPosition>>();

            userGenerator.StartGenerating();
        }

        #region Skeleton Events
        void skeletonCapbility_CalibrationComplete(object sender, CalibrationProgressEventArgs e)
        {
            if (e.Status == CalibrationStatus.OK)
            {
                skeletonCapbility.StartTracking(e.ID);
                joints.Add(e.ID, new Dictionary<SkeletonJoint, SkeletonJointPosition>());
            }
            else
            {
                if (skeletonCapbility.DoesNeedPoseForCalibration)
                {
                    poseDetectionCapability.StartPoseDetection(calibPose, e.ID);
                }
                else
                {
                    skeletonCapbility.RequestCalibration(e.ID, true);
                }
            }
        }

        void poseDetectionCapability_PoseDetected(object sender, PoseDetectedEventArgs e)
        {
            poseDetectionCapability.StopPoseDetection(e.ID);
            skeletonCapbility.RequestCalibration(e.ID, true);
        }

        void userGenerator_LostUser(object sender, UserLostEventArgs e)
        {
            joints.Remove(e.ID);
        }

        void userGenerator_NewUser(object sender, NewUserEventArgs e)
        {
            if (skeletonCapbility.DoesNeedPoseForCalibration)
            {
                poseDetectionCapability.StartPoseDetection(calibPose, e.ID);
            }
            else
            {
                skeletonCapbility.RequestCalibration(e.ID, true);
            }
        }
        #endregion

        #region Skeleton Drawing OpenNI
        public Bitmap DrawSkeletonImageBMP(Bitmap depthBitmap, DepthGenerator generator)
        {
            var skeletonBitmap = new Bitmap(depthBitmap.Width, depthBitmap.Height);
            Graphics g = Graphics.FromImage(skeletonBitmap);

            g.FillRectangle(System.Drawing.Brushes.Transparent, 0, 0, skeletonBitmap.Width, skeletonBitmap.Height);

            int[] users = this.userGenerator.GetUsers();
            foreach (int user in users)
            {
                Point3D com = this.userGenerator.GetCoM(user);
                com = generator.ConvertRealWorldToProjective(com);

                if (skeletonCapbility.IsTracking(user))
                {
                    DrawSkeleton(g, anticolors[user % ncolors], user, generator);
                }

            }
            g.Dispose();

            return skeletonBitmap;
        }

        private void GetJoint(int user, SkeletonJoint joint, DepthGenerator generator)
        {
            SkeletonJointPosition pos = this.skeletonCapbility.GetSkeletonJointPosition(user, joint);
            if (pos.Position.Z == 0)
            {
                pos.Confidence = 0;
            }
            else
            {
                pos.Position = generator.ConvertRealWorldToProjective(pos.Position);
            }
            this.joints[user][joint] = pos;
        }

        private void GetJoints(int user, DepthGenerator generator)
        {
            GetJoint(user, SkeletonJoint.Head, generator);
            GetJoint(user, SkeletonJoint.Neck, generator);

            GetJoint(user, SkeletonJoint.LeftShoulder, generator);
            GetJoint(user, SkeletonJoint.LeftElbow, generator);
            GetJoint(user, SkeletonJoint.LeftHand, generator);

            GetJoint(user, SkeletonJoint.RightShoulder, generator);
            GetJoint(user, SkeletonJoint.RightElbow, generator);
            GetJoint(user, SkeletonJoint.RightHand, generator);

            GetJoint(user, SkeletonJoint.Torso, generator);

            GetJoint(user, SkeletonJoint.LeftHip, generator);
            GetJoint(user, SkeletonJoint.LeftKnee, generator);
            GetJoint(user, SkeletonJoint.LeftFoot, generator);

            GetJoint(user, SkeletonJoint.RightHip, generator);
            GetJoint(user, SkeletonJoint.RightKnee, generator);
            GetJoint(user, SkeletonJoint.RightFoot, generator);
        }

        private void DrawLine(Graphics g, System.Drawing.Color color, Dictionary<SkeletonJoint, SkeletonJointPosition> dict, SkeletonJoint j1, SkeletonJoint j2)
        {
            Point3D pos1 = dict[j1].Position;
            Point3D pos2 = dict[j2].Position;

            if (dict[j1].Confidence == 0 || dict[j2].Confidence == 0)
                return;


            System.Windows.Media.Brush mycolor = jointColors[j1];
            var MyColor = ((SolidColorBrush)mycolor).Color;
            var jointColor = System.Drawing.Color.FromArgb(MyColor.A, MyColor.R, MyColor.G, MyColor.B);

            if (j1 == SkeletonJoint.Head)
            {
                //Draw Head
                g.DrawEllipse(new System.Drawing.Pen(System.Drawing.Color.Black, 10), pos1.X - 10, pos1.Y - 10, 25, 25);

                g.DrawLine(new System.Drawing.Pen(System.Drawing.Color.DarkMagenta, 4),
                        new System.Drawing.Point((int)pos1.X, (int)pos1.Y),
                        new System.Drawing.Point((int)pos2.X, (int)pos2.Y));
            }
            else if (j1 == SkeletonJoint.RightHip && j2 == SkeletonJoint.Torso)
            {
                g.DrawLine(new System.Drawing.Pen(System.Drawing.Color.DarkMagenta, 4),
                        new System.Drawing.Point((int)pos2.X, (int)pos1.Y - 30), //-30 serve para fazer o ângulo da anca
                        new System.Drawing.Point((int)pos2.X, (int)pos2.Y));

                var diff = pos1.Y - 30 - pos2.Y; //-30 serve para fazer o ângulo da anca
                LowerTorso = new Point3D((int)pos2.X, (int)diff, (int)pos2.Z);
            }
            else if (j1 == SkeletonJoint.LeftHip && j2 == SkeletonJoint.RightHip)
            {
                //Nesta altura já tenho dados no LowerTorso

                var tmp = dict[SkeletonJoint.Torso].Position.Y + LowerTorso.Y;

                g.DrawLine(new System.Drawing.Pen(System.Drawing.Color.DarkMagenta, 4),
                            new System.Drawing.Point((int)pos1.X, (int)pos1.Y),
                            new System.Drawing.Point((int)LowerTorso.X, (int)tmp));

                g.DrawLine(new System.Drawing.Pen(System.Drawing.Color.DarkMagenta, 4),
                            new System.Drawing.Point((int)pos2.X, (int)pos2.Y),
                            new System.Drawing.Point((int)LowerTorso.X, (int)tmp));

                //Draw LowerTorso union
                g.DrawEllipse(new System.Drawing.Pen(System.Drawing.Color.Black, 6), (int)LowerTorso.X - 2, (int)tmp - 2, 5, 5);
            }
            else
            {
                g.DrawLine(new System.Drawing.Pen(System.Drawing.Color.DarkMagenta, 4),
                        new System.Drawing.Point((int)pos1.X, (int)pos1.Y),
                        new System.Drawing.Point((int)pos2.X, (int)pos2.Y));
            }


            //draw line union
            g.DrawEllipse(new System.Drawing.Pen(System.Drawing.Color.Black, 6), pos1.X - 2, pos1.Y - 2, 5, 5);
            g.DrawEllipse(new System.Drawing.Pen(System.Drawing.Color.Black, 6), pos2.X - 2, pos2.Y - 2, 5, 5);
        }

        private void DrawSkeleton(Graphics g, System.Drawing.Color color, int user, DepthGenerator generator)
        {
            GetJoints(user, generator);
            Dictionary<SkeletonJoint, SkeletonJointPosition> dict = this.joints[user];

            //var p1 = dict[SkeletonJoint.Neck].Position;
            //var p2 = dict[SkeletonJoint.RightShoulder].Position;
            //var p3 = dict[SkeletonJoint.RightHand].Position;
            //angleString = string.Format("Angulo:{0}", GetAngle(p2, p1, p3));

            //currentAngle = GetAngle(p2, p1, p3);
            startingPoint = dict[SkeletonJoint.Torso].Position;

            DrawLine(g, color, dict, SkeletonJoint.Head, SkeletonJoint.Neck);
            DrawLine(g, color, dict, SkeletonJoint.Neck, SkeletonJoint.Torso);

            DrawLine(g, color, dict, SkeletonJoint.Neck, SkeletonJoint.LeftShoulder);
            DrawLine(g, color, dict, SkeletonJoint.LeftShoulder, SkeletonJoint.LeftElbow);
            DrawLine(g, color, dict, SkeletonJoint.LeftElbow, SkeletonJoint.LeftHand);

            DrawLine(g, color, dict, SkeletonJoint.Neck, SkeletonJoint.RightShoulder);
            DrawLine(g, color, dict, SkeletonJoint.RightShoulder, SkeletonJoint.RightElbow);
            DrawLine(g, color, dict, SkeletonJoint.RightElbow, SkeletonJoint.RightHand);

            DrawLine(g, color, dict, SkeletonJoint.RightHip, SkeletonJoint.Torso);

            DrawLine(g, color, dict, SkeletonJoint.LeftHip, SkeletonJoint.RightHip);

            DrawLine(g, color, dict, SkeletonJoint.LeftHip, SkeletonJoint.LeftKnee);
            DrawLine(g, color, dict, SkeletonJoint.LeftKnee, SkeletonJoint.LeftFoot);

            DrawLine(g, color, dict, SkeletonJoint.RightHip, SkeletonJoint.RightKnee);
            DrawLine(g, color, dict, SkeletonJoint.RightKnee, SkeletonJoint.RightFoot);


        }

        private static Dictionary<SkeletonJoint, System.Windows.Media.Brush> jointColors = new Dictionary<SkeletonJoint, System.Windows.Media.Brush>() { 
            {SkeletonJoint.Head, new SolidColorBrush(System.Windows.Media.Color.FromRgb(169, 176, 155))},
            {SkeletonJoint.LeftAnkle, new SolidColorBrush(System.Windows.Media.Color.FromRgb(169, 176, 155))},
            {SkeletonJoint.LeftCollar, new SolidColorBrush(System.Windows.Media.Color.FromRgb(168, 230, 29))},
            {SkeletonJoint.LeftElbow, new SolidColorBrush(System.Windows.Media.Color.FromRgb(200, 0,   0))},
            {SkeletonJoint.LeftFingertip, new SolidColorBrush(System.Windows.Media.Color.FromRgb(79,  84,  33))},
            {SkeletonJoint.LeftFoot, new SolidColorBrush(System.Windows.Media.Color.FromRgb(84,  33,  42))},
            {SkeletonJoint.LeftHand, new SolidColorBrush(System.Windows.Media.Color.FromRgb(255, 126, 0))},
            {SkeletonJoint.LeftHip, new SolidColorBrush(System.Windows.Media.Color.FromRgb(215,  86, 0))},
            {SkeletonJoint.LeftKnee, new SolidColorBrush(System.Windows.Media.Color.FromRgb(33,  79,  84))},
            {SkeletonJoint.LeftShoulder, new SolidColorBrush(System.Windows.Media.Color.FromRgb(33,  33,  84))},
            {SkeletonJoint.LeftWrist, new SolidColorBrush(System.Windows.Media.Color.FromRgb(77,  109, 243))},
            {SkeletonJoint.Neck, new SolidColorBrush(System.Windows.Media.Color.FromRgb(37,   69, 243))},
            {SkeletonJoint.RightAnkle, new SolidColorBrush(System.Windows.Media.Color.FromRgb(77,  109, 243))},
            {SkeletonJoint.RightCollar, new SolidColorBrush(System.Windows.Media.Color.FromRgb(69,  33,  84))},
            {SkeletonJoint.RightElbow, new SolidColorBrush(System.Windows.Media.Color.FromRgb(229, 170, 122))},
            {SkeletonJoint.RightFingertip, new SolidColorBrush(System.Windows.Media.Color.FromRgb(255, 126, 0))},
            {SkeletonJoint.RightFoot, new SolidColorBrush(System.Windows.Media.Color.FromRgb(181, 165, 213))},
            {SkeletonJoint.RightHand, new SolidColorBrush(System.Windows.Media.Color.FromRgb(71, 222,  76))},
            {SkeletonJoint.RightHip, new SolidColorBrush(System.Windows.Media.Color.FromRgb(245, 228, 156))},
            {SkeletonJoint.RightKnee, new SolidColorBrush(System.Windows.Media.Color.FromRgb(77,  109, 243))},

            {SkeletonJoint.RightShoulder, new SolidColorBrush(System.Windows.Media.Color.FromRgb(37,  249, 3))},
            {SkeletonJoint.RightWrist, new SolidColorBrush(System.Windows.Media.Color.FromRgb(217,  49, 43))},
            {SkeletonJoint.Torso, new SolidColorBrush(System.Windows.Media.Color.FromRgb(25,  105, 124))},
            {SkeletonJoint.Waist, new SolidColorBrush(System.Windows.Media.Color.FromRgb(150,  145, 225))}
        };

        #endregion

        #region Color Help Data
        private System.Drawing.Color[] colors = { System.Drawing.Color.Red, System.Drawing.Color.Blue, System.Drawing.Color.ForestGreen, System.Drawing.Color.Yellow, System.Drawing.Color.Orange, System.Drawing.Color.Purple, System.Drawing.Color.White };
        private System.Drawing.Color[] anticolors = { System.Drawing.Color.Green, System.Drawing.Color.Orange, System.Drawing.Color.Red, System.Drawing.Color.Purple, System.Drawing.Color.Blue, System.Drawing.Color.Yellow, System.Drawing.Color.Black };
        private int ncolors = 6;
        #endregion
    }
}
